#include <arduino/Arduino.h>
#include <avr/interrupt.h>

#include <sonar/srf05.h>
#include <sonar/srf04.h>
#include <servo/sg90.h>
#include <stepper/uln2003apg.h>

#include <utils/scope_timer.h>


sonar::srf05 sonar1;
sonar::srf04 sonar2;
servo::sg90* serv;
stepper::uln2003apg step_left;
stepper::uln2003apg step_right;

int pos = 90;
int diode_pin = 0;
bool left_to_right = true;
int forward_angle = 20;


void go_forward(void)
{
  step_right.set_motor_speed(-1.f);
  step_left.set_motor_speed(1.f);
}


void setup(void)
{
  Serial.begin(9600);

  sonar1.setup(12);
  sonar2.setup(2,1);

  serv = new servo::sg90(3);

  step_left.setup(8,9,10,11);
  step_right.setup(4,5,6,7);

  pinMode(diode_pin, OUTPUT);

  Serial.println("Hello !");
  delay(2000);
  Serial.println("Begin !");

  go_forward();
}


bool detect_ground(void)
{
  sonar2.loop();
//  Serial.print("Sonar ground: ");
//  Serial.println(sonar2.get_measure());

  if (sonar2.get_measure() <= 6)
    return true;

  return false;
}


void blink_led(void)
{
  digitalWrite(diode_pin, HIGH);
  delay(200);
  digitalWrite(diode_pin, LOW);
  delay(200);
}


bool detect_obstacle(void)
{
  sonar1.loop();

  unsigned long mesure = sonar1.get_measure();
//  Serial.println(mesure);

  if (mesure < 20)
    return true;

  return false;
}


int find_direction(void)
{
  int max_angle = 0;
  unsigned long max_distance = 0;

  for (int angle = 0; angle < 180; ++angle)
  {
    utils::scope_timer timer(20);

    serv->set_position(angle);
    sonar1.loop();
    unsigned long mesure = sonar1.get_measure();

    if (max_distance < mesure)
    {
      max_angle = angle;
      max_distance = mesure;
    }
  }

  serv->set_position(90);

  return max_angle;
}


void align_with_angle(int angle)
{
  static const int time_constant = 60;

  if (angle > 90)
  {
    utils::scope_timer timer((angle - 90) * time_constant);
    step_right.set_motor_speed(-1.f);
    step_left.set_motor_speed(0.f);
  }
  else
  {
    utils::scope_timer timer((90 - angle) * time_constant);
    step_right.set_motor_speed(0.f);
    step_left.set_motor_speed(1.f);
  }
}


void stop_robot(void)
{
  step_right.set_motor_speed(0);
  step_left.set_motor_speed(0);
}


void loop(void)
{
  utils::scope_timer timer(40);

  if (left_to_right)
  {
    pos += 2;

    if (pos >= 90 + forward_angle)
      left_to_right = false;
  }
  else
  {
    pos -= 2;

    if (pos <= 90 - forward_angle)
      left_to_right = true;
  }

  serv->set_position(pos);

  while (!detect_ground())
  {
    stop_robot();
    blink_led();
  }
  go_forward();

  if (detect_obstacle())
  {
    stop_robot();

    int direction = find_direction();
    align_with_angle(direction);
    go_forward();
  }
}
